/*
 * BeaconTask.java
 *
 * Created on April 15, 2008, 10:08 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package org.sunspotworld;

import com.sun.spot.peripheral.ChannelBusyException;
import com.sun.spot.peripheral.ChannelBusyException;
import org.sunspotworld.mydatawrappers.GPSPacket;
import org.sunspotworld.sensortasks.TimeKeeper;
import org.sunspotworld.sensortasks.SensorTask;
import org.sunspotworld.datawrappers.DataWrapperVector;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.peripheral.radio.IRadiogramProtocolManager;
import com.sun.spot.peripheral.radio.LowPan;
import com.sun.spot.peripheral.radio.RadioPacket;
import com.sun.spot.peripheral.radio.RadioPacket;
import com.sun.spot.peripheral.radio.RadioPolicy;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.util.IEEEAddress;
import com.sun.spot.util.IEEEAddress;
import com.sun.spot.util.Utils;
import com.sun.spot.util.Utils;
import com.sun.squawk.util.StringTokenizer;
import java.io.IOException;
import java.util.Random;
import org.sunspotworld.datawrappers.DataWrapperVector;
import org.sunspotworld.hardware.LEDManager;
import org.sunspotworld.hardware.LEDManager;
import org.sunspotworld.market.Bid;
import org.sunspotworld.market.Bid;
import org.sunspotworld.market.MarketAgent;
import org.sunspotworld.market.MarketAgent;
import org.sunspotworld.mydatawrappers.GPSPacket;
import org.sunspotworld.protocols.MyProtocols;
import org.sunspotworld.protocols.MyProtocols;
import org.sunspotworld.sensortasks.SensorTask;
import org.sunspotworld.sensortasks.TimeKeeper;

/**
 * A SensorTask that sends and receives radio beacons with other SPOTs to determin relative distances
 * @author Douglas McClusky
 */
public final class BeaconTask extends SensorTask {
    private static final int RANDOM_BACKOFF = 10;
    private static final int CONSTANT_BACKOFF = 2;
    private static final int BROADCAST_RETRIES = 5;
    private static final int PAYLOAD_SIZE = RadioPacket.MIN_PAYLOAD_LENGTH - IRadiogramProtocolManager.DATA_OFFSET;
    private static IRadioPolicyManager rpm = Spot.getInstance().getRadioPolicyManager();
    private static long stop;
    private EDemoBoard demoBoard = EDemoBoard.getInstance();
    private double [] lastReading = new double [3];
    private long readTime = 0;
    private static final float HDOP_CUTOFF = (float)7.0;
    private static final int POWER = 32; //7;
    private static Random random = new Random(new IEEEAddress(System.getProperty("IEEE_ADDRESS")).asLong());
    private boolean readingTaken = false;
    private static boolean hasGPS;
    private static byte[] beacon = new byte[PAYLOAD_SIZE];
    private static int size = 1;
    private int retries = 0;
    //private int[] powers = {-25,-12};
    
    /**
     * Creates a new instance of BeaconTask 
     * @param sample - The length of time to remain awake listening for beacons.
     */
    public BeaconTask(long sample) {
        super.sampleInterval = sample;
        demoBoard.initUART(4800, true);
        hasGPS = clearUART();
        if (hasGPS) {
            LEDManager.getInstance().setColor(7, LEDColor.TURQUOISE);
        } else {
    /**/    System.out.println("No GPS Found");
            LEDManager.getInstance().setColor(7, LEDColor.RED);
        }
        if (hasGPS) {
            MarketAgent.getInstance().addBid(new Bid(this, GPSPacket.SIZE, GPSPacket.SIZE));
        }//*/
        LEDManager.getInstance().turnOnLED(7);
    }
    
    private BeaconTask(long sample, long id) {
        super.sampleInterval = sample;
        super.id = id;
    }

    protected SensorTask copy() {
        SensorTask task = (SensorTask)(new BeaconTask(super.sampleInterval, super.id));
        task.setRepeat(super.repeating);
        return task;
    }

    private void readGPS() {
        readingTaken = false;
        while(!readingTaken) {
            LEDManager.getInstance().setColor(0, LEDColor.RED);
            LEDManager.getInstance().turnOnLED(0);
            readingTaken = takeReading();
            if (stop < TimeKeeper.getInstance().currentTimeMillis()) { 
                System.out.println("done");
                return;
            } 
        }
    }
    
    /**
     * Spawns a thread to listen for other SPOTs running Beacon tasks in this time slice
     * If SPOT is ranging, sends a radio beacon
     * If a GPS is connected to the EDemoBoard usart, gps information is encoded in the beacon
     */
    public void runTask() {
        stop = TimeKeeper.getInstance().currentTimeMillis() + super.sampleInterval;
        System.out.println("Listening");
        DataWrapperVector.getInstance().startNewNeighborRecord();
        Spot.getInstance().getRadioPolicyManager().policyHasChanged(TimeKeeper.getInstance(), RadioPolicy.ON);
        rpm.setOutputPower(POWER - 32);
        System.out.println("Preparing Beacon");
        if (Spot.getInstance().getPersistentProperty("Ranging").equals("true")) {
            broadcastBeacon();
        }
        if (hasGPS) {
            clearUART();
            readGPS();
        }
        stop = stop - TimeKeeper.getInstance().currentTimeMillis() - 5;
        if (stop > 0) {
            Utils.sleep(stop);
        }
        rpm.setOutputPower(0);
        Spot.getInstance().getRadioPolicyManager().policyHasChanged(TimeKeeper.getInstance(), RadioPolicy.OFF);
        System.out.println("Done Listening");
    }
    
    private void broadcastBeacon() {
        try {
            Utils.sleep(10);
            System.out.println("Sending Beacon...");
            LowPan.getInstance().sendBroadcast(MyProtocols.BEACON, beacon, 0, size, 0);
            System.out.println("Beacon Sent.");
        } catch (ChannelBusyException ex) {
            ex.printStackTrace();
            if (TimeKeeper.getInstance().currentTimeMillis() + RANDOM_BACKOFF + CONSTANT_BACKOFF < stop) {  //if there's enough time left
                //retry if channel busy
                Utils.sleep(random.nextInt(RANDOM_BACKOFF) + CONSTANT_BACKOFF);  // wait a random amount before retrying
                broadcastBeacon();
            }
        }
    }
    
    private boolean takeReading() {
        waitForMessage();
        String gpsData = this.readMessage();
        System.out.println(gpsData);
        StringTokenizer tokenizer = new StringTokenizer(gpsData, ",");
        String label = tokenizer.nextToken();
        System.out.println("Label: " + label);
        if (label.equals("$GPGGA")) {
            try {
                /*
                TimeKeeper.getInstance().updateTime(tokenizer.nextToken());
                /*/ tokenizer.nextToken(); //*/
                lastReading[0]=convert (Double.parseDouble(tokenizer.nextToken()));
                System.out.println("WHEE, READING!");
                System.out.println(lastReading[0]);
                if (tokenizer.nextToken().equals("S")) {
                    lastReading[0] *=-1;
                }
                System.out.println(lastReading[0]);
                lastReading[1]=convert (Double.parseDouble(tokenizer.nextToken()));
                System.out.println(lastReading[1]);
                if (tokenizer.nextToken().equals("W")) {
                    lastReading[1] *=-1;
                }
                System.out.println(lastReading[1]);
                if (Integer.parseInt(tokenizer.nextToken())>0) {
                    tokenizer.nextToken();
                    lastReading[2] = Double.parseDouble(tokenizer.nextToken());
                    System.out.println(lastReading[2]);
                    if (lastReading[2] <= HDOP_CUTOFF) {
                        //make packet
                        GPSPacket pkt = new GPSPacket(lastReading[0], lastReading[1], lastReading[2]);
                        System.out.println(GPSPacket.printRecordHeader());
                        System.out.println(pkt.printEntryLine());
                        DataWrapperVector.getInstance().add(pkt);
                        return true;
                    }
                }
            } catch (Exception e) {
                e.printStackTrace();
            }
        } 
        return false;        
    }
    //*
    private boolean clearUART() {
        try {
            System.out.println("Clearing UART buffer");
            boolean hadStuff = demoBoard.availableUART() > 0;
            while (demoBoard.availableUART() > 0) {
                try {
                    demoBoard.readUART();
                } catch (IOException ex) {
                    ex.printStackTrace();
                }
            }
            System.out.println("UART buffer cleared");
            return hadStuff;
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        System.out.println("UART Error");
        return false;
    }
    //*/
    private void waitForMessage() {
        byte val = 0;
        char dollarSign = '$';
        System.out.println("waiting...");
        while (val != dollarSign) {
            try {
                val = demoBoard.readUART();
            } catch (Exception e) {
                Utils.sleep(10);
            }
        }
        System.out.println("");
    }
    
    private String readMessage() {
        StringBuffer sBuff = new StringBuffer();
        sBuff.append('$');
        byte val = 0;
        System.out.println("receiving");
        //System.out.println(EDemoBoard.getInstance().availableUART());
        //System.out.println((char)val);
        while(true) {
            try {
                val = demoBoard.readUART();
                switch (val) {
                    case 10:
                        return sBuff.toString();
                    case (byte)('$'):
                        return readMessage();
                    default:
                        sBuff.append((char)val);
                }
            } catch (Exception e) {
                e.printStackTrace();
                Utils.sleep(10);
            }
        }
    }
    
    private double convert(double value) {
        value /= 100;
        int degrees = (int)Math.floor(value);
        value -= degrees;
        return value * 100/60 + degrees;
    }
    
}
